#include "clock/clock.h"
#include <clock/nslu2.h>
#include <sel4/sel4.h>

#include <mapping.h>

#include <cspace/cspace.h>

#define verbose 5
#include <sys/debug.h>
#include <sys/panic.h>


seL4_IRQHandler TimestampInt;
seL4_IRQHandler GPTimer0Int;

unsigned int TSBigEnd; // Counts the number of times the timer has overflowed

seL4_IRQHandler setUpInterrupt(seL4_CPtr interrupt_ep, int irq);
void setTIM0_int_period(unsigned int period);

typedef enum {
	Uninitialised =-1,
	Stopped =0,
	Running =1
}timer_state;

timer_state timerRunning = Uninitialised;

typedef struct{
	uint64_t time;
	seL4_CPtr client;
} timer;

timer *timerHeap = NULL;

int timerHeapIndex; // The index of the next location to insert to

int timerHeapInit(void){
	if (timerHeap != NULL){
		return 0;
	}
	timerHeap = malloc(sizeof(timer) * (MAX_TIMERS + 1));
	timerHeapIndex = 1;
	return timerHeap == NULL;
}

int timerHeapSize(void){
	return timerHeapIndex-1;
}

timer timerHeapTop(void){
	return timerHeap[1];
}

int timerHeapPush(timer toAdd){
	int index;
	timer swap;
	if (timerHeapIndex > MAX_TIMERS){
		return CLOCK_R_FAIL;
	}
	index = timerHeapIndex;
	timerHeap[index] = toAdd;

	while (index/2 && (timerHeap[index].time < timerHeap[index/2].time)){
		swap = timerHeap[index];
		timerHeap[index] = timerHeap[index/2];
		timerHeap[index/2] = swap;
		index /= 2;
	}

	timerHeapIndex++;
	return CLOCK_R_OK;
}

int timerHeapPop(void){
	timer swap;
	int index;
	int indextoswap;

	conditional_panic(timerHeapIndex <= 0, "timerHeapIndex must be greater than 0");
	if (timerHeapIndex == 1){
		return 1;
	} else if (timerHeapIndex == 2){
		timerHeapIndex--;
		return 0;
	}

	index = 1;
	timerHeap[index] = timerHeap[timerHeapIndex-1];
	timerHeapIndex--;

	while ((index*2   < timerHeapIndex && (timerHeap[index].time > timerHeap[index*2].time)) ||
		   (index*2+1 < timerHeapIndex && (timerHeap[index].time > timerHeap[index*2+1].time))){

		if (index*2+1 >= timerHeapIndex || (timerHeap[index*2].time < timerHeap[index*2+1].time)){
			indextoswap = index*2;
		} else {
			indextoswap = index*2+1;
		}
		swap = timerHeap[indextoswap];
		timerHeap[indextoswap] = timerHeap[index];
		timerHeap[index] = swap;
		index = indextoswap;
	}

	return 0;
}

void testHeap(void){
	timer a, b, c, d, e, f, g;
	a.time = 0;
	b.time = 1;
	c.time = 2;
	d.time = 3;
	e.time = 4;
	f.time = 5;
	g.time = 6;

	timerHeapPush(a);
	timerHeapPush(g);
	timerHeapPush(b);
	timerHeapPush(f);
	timerHeapPush(c);
	timerHeapPush(e);
	timerHeapPush(d);

	conditional_panic(timerHeapTop().time != a.time, "Failed on a");
	timerHeapPop();
	conditional_panic(timerHeapTop().time != b.time, "Failed on b");
	timerHeapPop();
	conditional_panic(timerHeapTop().time != c.time, "Failed on c");
	timerHeapPop();
	conditional_panic(timerHeapTop().time != d.time, "Failed on d");
	timerHeapPop();
	conditional_panic(timerHeapTop().time != e.time, "Failed on e");
	timerHeapPop();
	conditional_panic(timerHeapTop().time != f.time, "Failed on f");
	timerHeapPop();
	conditional_panic(timerHeapTop().time != g.time, "Failed on g");
	timerHeapPop();

	conditional_panic(timerHeapSize() != 0, "Heap must contain 0 elements at the end of test");

	dprintf(0, "Heap passed tests!\n");
}

int start_timer(const seL4_BootInfo* info, seL4_CPtr interrupt_ep){
	int i, err;
	seL4_CPtr  clock_start;
	unsigned int time;

	if(timerRunning == Uninitialised){

		//Initialise the times that the time-stamp timer has overflowed to 0
		TSBigEnd = 0;

		//locate the cap that covers the target addresses, assuming it is covered by exactly one frame cap
		for(i = 0 ; (i< MAX_NUM_DEVICE_REGIONS) && (info->deviceRegions[i].basePaddr != NSLU2_OSTS_PHYS_BASE);i++);

		conditional_panic(i >= MAX_NUM_DEVICE_REGIONS,"deviceRegion frames containing clock registers cannot be located\n");
		dprintf(1," region containing timer registers field found : %x - %x\n",info->deviceRegions[i].basePaddr,info->deviceRegions[i].basePaddr+ ((info->deviceRegions[i].frames.end - info->deviceRegions[i].frames.start)<< info->deviceRegions[i].frameSizeBits));

		conditional_panic(info->deviceRegions[i].frameSizeBits<6, "timer registers addresses occupy more than one cap\n");

		//record the frame address for mapping
		clock_start = info->deviceRegions[i].frames.start;

		//map it
		err = map_page(clock_start, seL4_CapInitThreadPD, NSLU2_OSTS_VIR_BASE, seL4_AllRights, 0);
		conditional_panic(err, "Failed to map clock registers\n");

		timerHeapInit();
	}

	//clear the time-stamp overflow interrupt bit
	OST_STATUS|= (0x1<<2);

	// set up timer 0 interrupts to occur every 100ms (100000us)
	// Has to set the upper 30 bits to the number of ticks, then set the bottom bit as true to activate the timer
	OST_TIM0_RL = (NSLU2_US2TICKS(100000) & (0xFFFFFFFC)) | (0x1);

	TimestampInt = setUpInterrupt(interrupt_ep, NSLU2_TIMESTAMP_IRQ);
	GPTimer0Int = setUpInterrupt(interrupt_ep, NSLU2_TIMER0_IRQ);

	time =  OST_TS;

	timerRunning = Running;

	testHeap();
	return 0;
}

int register_timer(uint64_t delay, seL4_CPtr client){
	timestamp_t ts;
	ts = time_stamp();
	dprintf (0, "registered timer {%llu us, %d} \n", delay + ts, client);

	//shorten the interrupt interval
	//note that the time_stamp is stll kept by the TS timer, so the accuracy of the incoming interrupt is only relevant to the waiting threads
	//shorten the interrupt inerval significantly(compared to the time it takes to reload) cannot decrease the quality of the service
	if((timerHeapSize() == 0 || (delay+ts) < timerHeapTop().time)){
		setTIM0_int_period((unsigned int) (delay/2));
	}

	//push the timestamp which when achieved the client will be waken up (in us)
	return timerHeapPush((timer) {(delay + ts), client});
}

void setTIM0_int_period(unsigned int period){

	if( period > TS_MAX_INT_PERIOD_US) return;

	//reload tim_0 with max(period,minimum period) and restart it
	if(period < TS_MIN_INT_PERIOD_US ){
		OST_TIM0_RL &=~ 0x1;
		OST_TIM0_RL = (NSLU2_US2TICKS(TS_MIN_INT_PERIOD_US) & (0xFFFFFFFC)) | (0x1);
		//dprintf(0,"timer interrupt period updated to %u us\n",TS_MIN_INT_PERIOD_US );
	}
	else {
		OST_TIM0_RL &=~ 0x1;
		OST_TIM0_RL = (NSLU2_US2TICKS(period) & (0xFFFFFFFC)) | (0x1);
		//dprintf(0,"timer interrupt period updated to %u us\n",period );
	}

	//clear interrupts
	OST_STATUS |= 1;
	seL4_IRQHandler_Ack(GPTimer0Int);
}


timestamp_t time_stamp(void){
	uint64_t ticks;
	ticks = (((long long) TSBigEnd) << 32) | OST_TS;
	return NSLU2_TICKS2US(ticks);
}

int stop_timer(void){
	if(timerRunning != Running) return CLOCK_R_UINT;

	//disable timer0, there is no need to disable the interrupt nor revoke the cap(s) nor unmap the frame
	OST_TIM0_RL &=~ 0x1;

	while(timerHeapSize() >0){
		dprintf(0,"client %u evicted because the driver is stopped\n", timerHeapTop().client);
		timerHeapPop();
	}

	timerRunning = Stopped;
	return CLOCK_R_OK;
}

void clock_interrupts(int irq){
	int err, ticks;
	// Handle time stamp interrupts
	if (irq & (1 << NSLU2_TIMESTAMP_IRQ)){
		TSBigEnd++;
		//clear the time-stamp timer interrupt bit
		OST_STATUS |= 0x4;
		err = seL4_IRQHandler_Ack(TimestampInt);
		conditional_panic(err, "Failed to acknowledge timer interrupt\n");
	}
	if (irq & (1<< NSLU2_TIMER0_IRQ)){
		ticks = OST_TS;
		//dprintf(0, "Timestamp: %llu ms\n", time_stamp()/1000);
		OST_STATUS |= 1;
		err = seL4_IRQHandler_Ack(GPTimer0Int);
		conditional_panic(err, "Failed to acknowledge timer interrupt\n");

		while (timerHeapSize()>0 && (timerHeapTop().time < time_stamp())){
			dprintf(0, "We should wake up {%lld us, %d} since it's  %lld us : time difference : %lld us\n", timerHeapTop().time, timerHeapTop().client, time_stamp(),time_stamp() -timerHeapTop().time );
			//remove the item
			timerHeapPop();
		}
	}
}

/*
 * A helper function that sets up an interrupt of number irq, binds it to interrupt_ep
 * Should only be called during boot time
 */
seL4_IRQHandler setUpInterrupt(seL4_CPtr interrupt_ep, int irq){
	//acquire the irq_handler cap
	seL4_IRQHandler irq_handler = cspace_irq_control_get_cap(cur_cspace, seL4_CapIRQControl, irq);

	//binds it to the ep
	int err = seL4_IRQHandler_SetEndpoint(irq_handler, interrupt_ep);
	conditional_panic(err, "Failed to bind clock interrupt to the endpoint or failed to acquired irq_handler cap\n");

	//ack interrupt to clear spurious pending interrupts
	err = seL4_IRQHandler_Ack(irq_handler);
	conditional_panic(err, "Failed to acknowledge timer interrupt\n");
	return irq_handler;
}
